/*
 * @Description: 闭环检测流程文件
 * @Author: Sang Hao
 * @Date: 2021-10-18 16:12:26
 * @LastEditTime: 2021-10-27 14:07:03
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/loop_closing/loop_closing_flow.hpp"

namespace lidar_slam {
/**
 * @description: 句柄，将发布话题订阅话题都包括进句柄中
 * @param  {*}
 * @return {*}
 */
LoopClosingFlow::LoopClosingFlow(ros::NodeHandle& nh) {
	// publisher   // "map"为frame_id
	loop_pose_pub_ptr_ = std::make_shared<LoopPosePublisher>(nh, "/loop_pose", "map", 100);
	// sublisher
	key_frame_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_frame", 100000);
	// loop closing // 调用默认构造函数即可
	loop_closing_ptr_ = std::make_shared<LoopClosing>();
}
/**
 * @description: public 保留的对外接口
 * @param  {*}
 * @return {*}
 */
bool LoopClosingFlow::Run() {
	if (!ReadData()) {
		return false;
	}

	while (HasData()) {
		if (!ValidData()) {
			continue;
		}
		loop_closing_ptr_->Update(current_key_frame_);
		PublishData();
	}
	return true;	
}

/**
 * @description: 调用功能函数里的ParseData函数，将数据读入key_frame_buff_
 * @param  {*}
 * @return {*}
 */
bool LoopClosingFlow::ReadData() {
	// 解析数据，也就是将新订阅得到的数据添加到key_frame_buff_中去
	key_frame_sub_ptr_->ParseData(key_frame_buff_);
	return true;
}
/**
 * @description: 直接根据size判断key_frame_buff_中是否有数据
 * @param  {*}
 * @return {*}
 */
bool LoopClosingFlow::HasData() {
	if (key_frame_buff_.size() == 0) {
		return false;
	}
	return true;
}
/**
 * @description: 判断数据是否有效，是否丢了数据，这里暂定统统有效
 * @param  {*}
 * @return {*}
 */
bool LoopClosingFlow::ValidData() {
	current_key_frame_ = key_frame_buff_.front();
	key_frame_buff_.pop_front();
	return true;
}
/**
 * @description: 发布数据，一旦出现回环则发布当前的回环数据loop_pose
 * @param  {*}
 * @return {*}
 */
bool LoopClosingFlow::PublishData() {
	// 一旦出现回环，则发布
	if (loop_closing_ptr_->HasNewLoopPose()) {
		loop_pose_pub_ptr_->Publish(loop_closing_ptr_->GetCurrentLoopPose());
	}
	return true;
}
}